#include "Focus.h"
#include <assert.h>

Focus::Focus()
{
	
}

Focus::~Focus()
{

}


cv::Point2f Focus::getMaskCenter(cv::Mat &mat)
{	
	cv::Point2f point = {};
	if (!mat.empty()) {
		int x = mat.cols, y = mat.rows;
		point = cv::Point2f(x / 2, y / 2);
		maskcenter_ = point;
	}
	return point;
}

int Focus::preprocess(cv::Mat& mat, Attribute& attr)
{	
	if (mat.empty()) {
		return -1;
	}
	if (mat.channels() == 3) {
		auto clone = mat.clone();
		cv::cvtColor(clone, mat, cv::COLOR_BGR2GRAY);
	}

	return 0;
}

int Focus::findMaxOutline(cv::Mat &mat)
{	
	cv::findContours(mat, contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
	
	if (contours_.empty()) {
		return -1;
	}

	size_t maxContourIdx = -1;
	double maxArea = 0;

	for (size_t i = 0; i < contours_.size(); ++i) {
	double area = cv::contourArea(contours_[i]);
		if (area > maxArea) {
			maxArea = area;
			maxContourIdx = i;
		}
	}
	if (maxContourIdx == -1) {
		return -2;
	}

	maxcontour_ = contours_[maxContourIdx];

	return 0;
}

int Focus::findMaxOutline(cv::Mat& mat, Attribute& attr) {

	cv::findContours(mat, contours_, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

	std::vector<std::vector<cv::Point>> result;
	if (contours_.empty()) {
		return -1;
	}

	if (attr.type == Shapetype::ShapeNone) {
		return -2;
	}

	if ((attr.type == Circle && attr.radius == -1.0) || 
		(attr.type == Rect && (attr.width == -1.0 || attr.height == -1.0))) {
		return -3;
	}

	for (size_t i = 0; i < contours_.size(); ++i) {
		auto contour = contours_[i];
		Params param;
		fit(mat, attr, contour, param, result);
	}

	contours_ = std::move(result);

	if (attr.type == Circle) {
		std::sort(contours_.begin(), contours_.end(), [this, attr](const auto& u, const auto& v) -> bool {
			return std::abs(circlemap_[u] - attr.radius) < std::abs(circlemap_[v] - attr.radius); 
			});
	}

	if (attr.type == Rect) {
		std::sort(contours_.begin(), contours_.end(), [this, attr](const auto& u, const auto& v) -> bool {
			return (std::abs(rectmap_[u].first - attr.width) < std::abs(rectmap_[v].first - attr.width)) && (std::abs(rectmap_[u].second - attr.height) < std::abs(rectmap_[v].second - attr.height));
			});
	}

	
	return 0;
}

int Focus::draw(cv::Mat& mat, Attribute& attr) {
	int res = -3;
	switch (attr.type) {
	case Circle:
		res  = drawCircle(mat);
		break;
	case Rect:
		res = drawRect(mat);
		break;
	default:
		break;
	}

	return res;
}

cv::Point2f Focus::getMaxOutlineCenter(Attribute& attr) const
{   
	switch (attr.type) {
	case Circle:
		return cv::Point2f(shape.Circle.x, shape.Circle.y);
	case Rect:
		return cv::Point2f(shape.Rect.center.x, shape.Rect.center.y);
	default:
		break;
	}
	return {};
}

int Focus::drawCross(cv::Mat &mat, cv::Point2f center, const size_t crosslen)
{	
	if (mat.empty()) {
		return -1;
	}
	if (center.x == 0 && center.y == 0) {
		return -2;
	}

	cv::line(mat,
		cv::Point(center.x - crosslen, center.y),
		cv::Point(center.x + crosslen, center.y),
		cv::Scalar(0, 0, 255), 2);
	cv::line(mat,
		cv::Point(center.x, center.y - crosslen),
		cv::Point(center.x, center.y + crosslen),
		cv::Scalar(0, 0, 255), 2);

	return 0;
}

double Focus::CalculatePTOPDistance(cv::Point2f center1)
{	
	double res = -1.0;
	if (center1.x == 0 && center1.y) {
		return res;
	}
	double x = center1.x, y = center1.y;
	double res_x = std::pow(x - maskcenter_.x, 2), res_y = std::pow(y - maskcenter_.y, 2);
	res = std::sqrt(res_x + res_y);
	return res;
}

double Focus::CalculatePTOPDistance(cv::Point2f center1, cv::Point2f center2)
{
	double res = -1.0;
	if (center1.x == 0 && center1.y) {
		return res;
	}
	double x = center1.x, y = center1.y;
	double x_ = center2.x, y_ = center2.y;
	double res_x = std::pow(x - x_, 2), res_y = std::pow(y - y_, 2);
	res = std::sqrt(res_x + res_y);
	return res;
}

bool Focus::isCover(double distance, const double threshold)
{
	return distance != -1 && 0 <= threshold && distance < threshold;
}

std::vector<std::vector<cv::Point>> Focus::getContours() const
{
	return contours_;
}

int Focus::drawCircle(cv::Mat &mat)
{	
	if (mat.empty()) {
		return -1;
	}
	if (contours_.empty()) {
		return -2;
	}

	maxcontour_ = contours_[0];

	float radius = 0.0;
	cv::minEnclosingCircle(maxcontour_, 
		shape.Circle, 
		radius);
	cv::circle(mat, shape.Circle, 
		radius, 
		cv::Scalar(0, 255, 0), 2);

	return 0;
}

int Focus::drawRect(cv::Mat &mat)
{
	if (mat.empty()) {
		return -1;
	}
	if (contours_.empty()) {
		return -2;
	}

	maxcontour_ = contours_[0];

	shape.Rect = cv::minAreaRect(maxcontour_);

	cv::Point2f rectPoints[4];
	shape.Rect.points(rectPoints);
	for (int i = 0; i < 4; ++i) {
		cv::line(mat, rectPoints[i], rectPoints[(i + 1) % 4], cv::Scalar(255, 0, 0), 2);	
	}

	return 0;
}

int Focus::fit(cv::Mat& mat, Attribute& attr, std::vector<cv::Point>& contour, Params& param, std::vector<std::vector<cv::Point>> &result)
{
	switch (attr.type)
	{
	case Circle:
		cv::minEnclosingCircle(contour,
			shape.Circle,
			param.radius);
		if (param.radius < std::abs(attr.radius - attr.threshold)) result.emplace_back(contour);
		circlemap_[contour] = param.radius;
		break;
	case Rect:
		shape.Rect = cv::minAreaRect(contour);
		if ((shape.Rect.size.width < std::abs(attr.width - attr.threshold)) && (shape.Rect.size.height < std::abs(attr.height - attr.threshold))) result.emplace_back(contour);
		rectmap_[contour] = { shape.Rect.size.width, shape.Rect.size.height };
		break;
	default:
		break;
	}
		
	return 0;
}







